/**
 * @file utils.c
 * @brief 
 * @author Luke (AiNuozhou@bupt.edu.cn)
 * @version 1.0
 * @date 2021-11-01
 * 
 * @copyright Copyright (c) 2021  北京邮电大学
 * 
 * @par 修改日志:
 * @date 2021-11-01 
 * @author Luke (AiNuozhou@bupt.edu.cn)
 */

#include "utils.h"
#include "oslib.h"
void PID_init()
{
}

float PID_Release(PID_s *PID, float target, float now)
{
  float err;
  float err_dt;
  float result;

  err = target - now;
  err_dt = err - PID->last_err;

  err_dt *= 0.384f;
  err_dt += PID->last_d * 0.615f; //低通滤波

  PID->last_err = err;

  PID->i += err * PID->I_TIME; // 积分量

  LIMIT(PID->i, PID->i_max); // 限制积分量大小
  PID->last_d = err_dt;

  result = err * PID->KP + err_dt * PID->KD + PID->i * PID->KI;
  LIMIT(result, PID->output_max);
  return result;
}

void reset_PID(PID_s *s)
{
  s->i = 0;
  s->last_err = 0;
  s->last_d = 0;
}

void print_PID(PID_s *s)
{
  uprintf("kp = %f ",s->KP);
  uprintf("ki = %f ",s->KI);
  uprintf("kd = %f \n\r",s->KD);
  uprintf("output_max = %f ",s->output_max);
  uprintf("I_TIME = %f ",s->I_TIME);
  uprintf("i_max = %f \n\r",s->i_max);
}

float AngleLimit180(float angle)
{
  while (angle > 180)
  {
    angle -= 360;
  }
  while (angle <= -180)
  {
    angle += 360;
  }
  return angle;
}

float AngleLimitPI(float angle)
{
  while (angle > PI)
  {
    angle -= 2 * PI;
  }
  while (angle <= -PI)
  {
    angle += 2 * PI;
  }
  return angle;
}

float AngleLimit2PI(float angle)
{
  while (angle > 2 * PI)
  {
    angle -= 4 * PI;
  }
  while (angle <= -2 * PI)
  {
    angle += 4 * PI;
  }
  return angle;
}

/**
 * @brief 两角差值，限制在[0,pi]
 * 
 */
float AngleLimitDiff(float a, float b)
{
  float out = a - b;
  return AngleLimitPI(out);
}

float AngleBetweenPoints(float start_x, float start_y, float end_x, float end_y)
{
  float angle = 0;
  if (fabs(start_x - end_x) < 1e-4) // 浮点数不能直接比较大小
  {
    if (fabs(start_y - end_y) < 1e-4)
      angle = 0;
    else if (start_y < end_y)
      angle = PI / 2;
    else //(start_y > end_y)
      angle = -PI / 2;
  }
  else
  {
    angle = atan2f(end_y - start_y, end_x - start_x);
  }
  return angle;
}

/**
 * @brief 坐标系矩阵变换，
 * @param now 全场定位返回坐标
 * @param now_in_target 全场定位坐标相对于世界坐标的位置，角度为X轴偏角，逆时针为正
 * @param target 最终坐标
*/
void Vega_CoordinateTransform(float now[3], float now_in_target[3], float target[3])
{
  float c = cos(-now_in_target[2]);
  float s = sin(-now_in_target[2]);
  target[0] = now[0] * c + now[1] * s - now_in_target[0];
  target[1] = now[0] * (-s) + now[1] * c - now_in_target[1];
  target[2] = AngleLimitPI(now[2] - now_in_target[2]);
}
/*************************************
 * @brief: 计算直线方程
 * @param {Point2D_s} p1
 * @param {Point2D_s} p2
 * @return {*}
*************************************/
Line2D_s calculate_line_equation(Point2D_s p1, Point2D_s p2)
{
  Line2D_s line;
  if (p1.x != p2.x && p1.y != p2.y)
  {
    line.A = p2.y - p1.y;
    line.B = p1.x - p2.x;
    line.C = p1.y * p2.x - p1.x * p2.y;
    line.k = (p2.y - p1.y) / (p2.x - p1.x);
    line.b = (p1.y * p2.x - p1.x * p2.y) / (p2.x - p1.x);
  }
  else if (p1.x == p2.x && p1.y != p2.y)
  {
    line.A = 1;
    line.B = 0;
    line.C = -p1.x;
    line.k = INFINITY;
    line.b = 0;
  }
  else if (p1.y == p2.y && p1.x != p2.x)
  {
    line.A = 0;
    line.B = 1;
    line.C = -p1.y;
    line.k = 0;
    line.b = p1.y;
  }
  else
  {
    line.A = 0;
    line.B = 0;
    line.C = 0;
    line.k = 0;
    line.b = 0;
  }
  return line;
}

float calculate_point_to_line_distance(Point2D_s p, Line2D_s line)
{
  return (float)(fabs((line.A * p.x + line.B * p.y + line.C) / sqrt(line.A * line.A + line.B * line.B)));
}

float calculate_line_line_angle(Line2D_s line1, Line2D_s line2)
{
  return (float)(acos((line1.A * line2.A + line1.B * line2.B) / sqrt((line1.A * line1.A + line1.B * line1.B) * (line2.A * line2.A + line2.B * line2.B))));
}

void get_tick(Time_s *time)
{
  time->last_tick = time->tick;
  time->tick = osKernelGetTickCount();
  time->delta_tick = time->tick - time->last_tick;
  time->s = time->tick / 1000.0;
}

void reset_time(Time_s *time)
{
  time->tick = 0;
  time->last_tick = 0;
  time->s = 0;
  time->delta_tick = 0;
}

